#include "3DAlgo/shadow3d.h"
#include "qgl.h"
#include <GL/glu.h>
#include "stdlib.h"
#define GRID_SIZE 70
#define TOTAL  GRID_SIZE*GRID_SIZE*GRID_SIZE
#define qMAX 4
#define A .5
#define B 2
#define C 2
using namespace std;

Shadow3D::Shadow3D()
{
    m_grid = new float[TOTAL];
    m_max = 500;
    m_quadric = gluNewQuadric();
    for(int i =0; i< TOTAL; i++){
        m_grid[i] = 0;
    }
}

Shadow3D::~Shadow3D(){
    delete[] m_grid;
}

void Shadow3D::convertToIndex(float3 loc, int &x, int &y, int &z){
    if(loc.x >500 || loc.y > 500 || loc.z > 500) cout<<"out of bounds location(too high)"<<endl;
    if(loc.x <0 || loc.y <0 || loc.z <0) cout<<"out of bounds location (neg)"<<endl;

    x = (int) (loc.x / m_max * GRID_SIZE + 0.5);
    y = (int) (loc.y / m_max * GRID_SIZE + 0.5);
    z = (int) (loc.z / m_max * GRID_SIZE + 0.5);

}

void Shadow3D::indexToXY(int x, int y, int z, float3 &loc){
    loc.x = ((float) x) * m_max / (float) GRID_SIZE;
    loc.y = ((float) y) * m_max / (float) GRID_SIZE;
    loc.z = ((float) z) * m_max / (float) GRID_SIZE;
}

void Shadow3D::draw(){
    float size = m_max / (float) GRID_SIZE;
    glMatrixMode(GL_MODELVIEW);
    glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
    glDisable(GL_DEPTH_TEST);
    float3 loc;
    for(int i = 0; i< GRID_SIZE; i++){
        for(int j =0; j<GRID_SIZE; j++){
            for(int k = 0; k<GRID_SIZE; k++){
                indexToXY(i,j,k,loc);
                glColor4f(1.0,0,0,m_grid[i + j * GRID_SIZE + k*GRID_SIZE*GRID_SIZE]);
                glBegin(GL_QUADS);
                    glVertex3f(loc.x - size/2.0, loc.y- size/2.0, loc.z- size/2.0);
                    glVertex3f(loc.x + size/2.0, loc.y- size/2.0, loc.z- size/2.0);
                    glVertex3f(loc.x + size/2.0, loc.y+ size/2.0, loc.z- size/2.0);
                    glVertex3f(loc.x- size/2.0, loc.y+ size/2.0, loc.z- size/2.0);
                glEnd();
            }
        }
    }
    glDisable(GL_BLEND);
    glEnable(GL_DEPTH_TEST);
    glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);
}

void Shadow3D::populateGrid(Bud *bud){
    delete[] m_grid;
    m_grid = new float[TOTAL];
    for(int i=0; i<TOTAL; i++){
        m_grid[i] = 0.0;
    }
    this->recurPopulate(bud);
}

void Shadow3D::recurPopulate(Bud *bud){
    int sx,sy,sz, ind;
    if(!bud->term){
        convertToIndex(bud->end,sx,sy,sz);
        for(int q = 0; q< qMAX; q++){
            for(int p = -q; p<=q; p++){
                for(int o = -q; o<=q;o++ ){
                    if(sx + o < GRID_SIZE && sx + 0 >= 0 && sy - q >0 && sz + p < GRID_SIZE && sz + p >=0){
                        ind = (sx + o)  + GRID_SIZE*(sy - q) + GRID_SIZE*GRID_SIZE*(sz+p);
                        m_grid[ind] += A* pow((double) B, (double) -q);
                    }
                }
            }
        }
    }
    else
        recurPopulate(bud->term);

    if(!bud->aux){
        convertToIndex(bud->end,sx,sy,sz);
        for(int q = 0; q< qMAX; q++){
            for(int p = -q; p<=q; p++){
                for(int o = -q; o<=q;o++ ){
                    if(sx + o < GRID_SIZE && sx + 0 >= 0 && sy + q < GRID_SIZE && sz + p < GRID_SIZE && sz + p >=0){
                        ind = (sx + o)  + GRID_SIZE*(sy - q) + GRID_SIZE*GRID_SIZE*(sz+p);
                        m_grid[ind] += A* pow((double) B, (double) -q);
                    }
                }
            }
        }
    }
    else
        recurPopulate(bud->aux);
}

void Shadow3D::queryQ(Bud *bud){
    int x,y,z, tindx;
    convertToIndex(bud->end,x,y,z);
    int ind = x + GRID_SIZE*y + GRID_SIZE*GRID_SIZE*z;
    float3 best(0,1,0);
    float min = 100.0;
    if(!bud->aux){
        bud->aQ = max(C- m_grid[ind] + A, 0.0);
        for(int i = -1; i<2; i++){
            for(int j = -1 ; j<2; j++){
                for(int k= -1; k<2; k++){
                    if(x+i >= 0 && x+i < GRID_SIZE && y+j >= 0 && y+j < GRID_SIZE && z+k >= 0 && z+k < GRID_SIZE){
                        tindx = (x+i)+ GRID_SIZE*(y+j) + GRID_SIZE*GRID_SIZE*(z+k);
                        if(m_grid[tindx] < min && (i!=0 || j!=0 || k!=0)){
                            min = m_grid[tindx];
                            best = float3(x+i, y+j, z+k);
                        }
                        else if(m_grid[tindx] == min && rand()%100 < 50 && (i!=0 || j!=0 || k!=0)){
                            min = m_grid[tindx];
                            best = float3(x+i, y+j, z+k);
                        }
                    }
                }
            }
        }
        float3 dir(best.x - x, best.y - y, best.z - z);
        dir.normalize();
        bud->aV = dir;
    }
    else
        queryQ(bud->aux);

    if(!bud->term){
        bud->tQ = max(C- m_grid[ind] + A, 0.0);
        for(int i = -1; i<2; i++){
            for(int j = -1 ; j<2; j++){
                for(int k= -1; k<2; k++){
                    if(x+i >= 0 && x+i < GRID_SIZE && y+j >= 0 && y+j < GRID_SIZE && z+k >= 0 && z+k < GRID_SIZE){
                        tindx = (x+i)+ GRID_SIZE*(y+j) + GRID_SIZE*GRID_SIZE*(z+k);
                        if(m_grid[tindx] < min && (i!=0 || j!=0 || k!=0)){
                            min = m_grid[tindx];
                            best = float3(x+i, y+j, z+k);
                        }
                        else if(m_grid[tindx] == min && rand()%100 < 50 && (i!=0 || j!=0 || k!=0)){
                            min = m_grid[tindx];
                            best = float3(x+i, y+j, z+k);
                        }
                    }
                    else{
                        cout<<"out of bounds in Query"<<endl;
                    }
                }
            }
        }
        float3 dir(best.x - x, best.y - y, best.z - z);
        dir.normalize();
        bud->tV = dir;
    }
    else
        queryQ(bud->term);
}





